#include "Obstacles.h"


void Obstacles::processLaserData(const sensor_msgs::LaserScan& scan) {
    obstacles.clear();
    int flag = 0;
    double noise_threshold = 0.5; // 设置噪声阈值，允许的最大距离差异

    for (size_t i = 0; i < scan.ranges.size() - 1; ++i) {
        double dis = scan.ranges[i];
        double angle = scan.angle_min + scan.angle_increment * i;

        if (dis > scan.range_min && dis < scan.range_max && dis <= 4.0) {
            // 检查是否为噪声
            /* if (i > 0 && abs(dis - scan->ranges[i - 1]) > noise_threshold && i < scan->ranges.size() - 1 && abs(dis - scan->ranges[i + 1]) < noise_threshold) {
                continue; // 跳过此点，认为是噪声
            } */
            // 调整角度，使其相对于正前方
            double adjusted_angle = angle + (M_PI / 2); // 假设正前方是0度，背后是180度
            double x = dis * cos(angle) ;
            double y = dis * sin(angle);
            if(flag%40 == 0 || abs(dis - scan.ranges[i + 1]) > 0.2)
            {
                obstacles.push_back(std::make_pair(x, y));
                if(abs(dis - scan.ranges[i + 1]) > 0.2)flag = 0;
            } 
            flag++;
        }
    }
}

void Obstacles::transformToMapCoordinates(double x, double y, const std::vector<std::pair<double, double>>& xy_points) {
    double cos_yaw = cos(vehicle_yaw);
    double sin_yaw = sin(vehicle_yaw);

    // 计算障碍物在地图上的坐标
    double map_x = vehicle_x + (x * cos_yaw - y * sin_yaw);
    double map_y = vehicle_y + (x * sin_yaw + y * cos_yaw);

    // 存储障碍物坐标
    std::pair<double, double> point(map_x, map_y);

    // 检查路径是否为空
    if (!xy_points.empty()) {
        // 找到路径上最近的点
        PointAndIndex closest_point = obs_projection.match_and_projection_point(point, xy_points);

        // 根据索引获取路径点与下一个路径点形成的向量
        if (closest_point.index < xy_points.size() - 1) {
            auto next_point = xy_points[closest_point.index + 1];
            double path_dx = next_point.first - closest_point.x;
            double path_dy = next_point.second - closest_point.y;

            // 计算障碍物点与路径点形成的向量
            double obstacle_dx = map_x - closest_point.x;
            double obstacle_dy = map_y - closest_point.y;

            // 计算叉积
            double cross_product = path_dx * obstacle_dy - path_dy * obstacle_dx;

            // 计算距离
            double distance = sqrt(pow(obstacle_dx, 2) + pow(obstacle_dy, 2));

            // 如果障碍物在路径的右边或距离小于等于1米，则保留该障碍物点
            if (cross_product >= 0 || distance <= 1.0 ||  distance >= 1.3) {
                map_coordinates.push_back(point);
                obstacles_manager.AddObstaclesPoint(point);

                // 记录投影点
                obstacles_projection = closest_point;
                // std::cout <<  "obstacles_projection.index: " << obstacles_projection.index << std::endl;
                obstacles_projections.push_back(obstacles_projection);
            }
        }
    }
}


std::vector<PointAndIndex> Obstacles::getObstaclesProjections() {
    return obstacles_projections;
}

std::vector<myObstacle> Obstacles::getObstaclesPoints() {
    std::vector<myObstacle> obstacles_ptr;
    int obstacleCount = 0;
    for (const auto& coord : map_coordinates) {
    // 这里创建 myObstacle 对象，并使用 map_coordinates 的值。
    obstacles_ptr.push_back(myObstacle(coord.first, coord.second));
    obstacleCount++;
    }
    return obstacles_ptr;
}

void Obstacles::visualizeObstacles(const std::vector<myObstacle>& point) {
    visualization_msgs::Marker points;
    points.header.frame_id = "map";  // 设置参考系为地图
    points.header.stamp = ros::Time::now();
    points.ns = "Obstacles";
    points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = 1.0;

    points.id = 0;

    points.type = visualization_msgs::Marker::POINTS;

    // 设置点的大小
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // 设置颜色 (RGBA)
    points.color.r = 1.0f;
    points.color.g = 0.0f;
    points.color.b = 0.0f;
    points.color.a = 1.0;

    // 添加所有障碍物点到 marker
    for (const auto& coord : point) {
        geometry_msgs::Point p;
        p.x = coord.x;
        p.y = coord.y;
        p.z = 0;
        points.points.push_back(p);
    }

    marker_pub_.publish(points);
}


// 激光数据回调函数
void Obstacles::laser(const sensor_msgs::LaserScan& scan, const nav_msgs::Odometry& odom, const std::vector<std::pair<double, double>>& xy_points) {
    // 调用processLaserData函数处理激光数据
    map_coordinates.clear();
    sensor_msgs::LaserScan scan_ = scan;
    vehicle_x = odom.pose.pose.position.x;
    vehicle_y = odom.pose.pose.position.y;
    vehicle_yaw = tf::getYaw(odom.pose.pose.orientation);
    obstacles_projections.clear();
    std::vector<std::pair<double, double>> points = xy_points;
    processLaserData(scan_);
    int obstacleCount = 0;
    for (const auto& obstacle : obstacles) {
        transformToMapCoordinates(obstacle.first, obstacle.second, points);
        obstacleCount++;
    }
}

Obstacles::Obstacles() {
    marker_pub_ = nh.advertise<visualization_msgs::Marker>("obs_visualization", 1);  // 初始化可视化 publisher
}

Obstacles::~Obstacles(){}

